/*
	NounShipControl.cpp
	(c)2000 Palestar Inc, Richard Lyle
*/


#include "Debug/Assert.h"
#include "Debug/Trace.h"
#include "Debug/DebugOutput.h"
#include "Debug/Profile.h"
#include "Math/Helpers.h"
#include "System/Messages.h"
#include "World/NounZoneTarget.h"
#include "VerbBreakOrbit.h"
#include "VerbEnterOrbit.h"
#include "GadgetBeamWeapon.h"
#include "GadgetShield.h"
#include "NounStructure.h"
#include "NounNebula.h"
#include "NounShip.h"
#include "NounJumpGate.h"
#include "GameContext.h"
#include "Resource.h"

//----------------------------------------------------------------------------

static Constant			USE_JUMP_DISTANCE( "USE_JUMP_DISTANCE", 2000.0f );					// when moving to a target, at what range do we use the jump drives
static Constant			CHECK_JUMP_INTERVAL( "CHECK_JUMP_INTERVAL", 10.0f );				// how often to check for a jump collision

static const float		MAX_SHIP_ROLL = PI / 4;												// maximum roll when turning
static const float		MIN_SHIP_ROLL = -MAX_SHIP_ROLL;
const int				SHIP_SYNC_RATE = TICKS_PER_SECOND * 30;								// how often is a VerbSyncShip() sent 
const int				SHIP_UNLOCK_RATE = TICKS_PER_SECOND * 5;							// how often to call unlockDynamics() 

//----------------------------------------------------------------------------

#if defined(_DEBUG)
#define ENABLE_CONTROL_LOG
#endif

// define to non-zero to enable ship pitching when changing Z
#define ENABLE_SHIP_PITCH		0

#if defined(ENABLE_CONTROL_LOG)
static DebugOutput CONTROL_LOG( DebugOutput::RF_LOGFILE|DebugOutput::RF_DEBUGGER|DebugOutput::RF_RESET_LOGFILE, "Control" );
#endif

//----------------------------------------------------------------------------

inline bool HeadingEpsilon( float v1, float v2 )
{
	return fabs( v1 - v2 ) >= (PI / 360);
}

inline bool VelocityEpsilon( float v1, float v2 )
{
	return fabs( v1 - v2 ) >= 0.05f;
}

//----------------------------------------------------------------------------

bool NounShip::controlHeadingVelocity( float fHeading, float fVelocity )
{
	if ( flags() & FLAG_CONTROL_DISABLED )
		return false;		// ship control is disabled
	// now set the new heading and velocity
	m_fLastSetHeading = m_fSetHeading = NormalizeAngle( fHeading );
	m_fLastSetVelocity = m_fSetVelocity = fVelocity;
	m_fLastYaw = m_fYaw = 0.0f;

	return true;
}

bool NounShip::controlYawVelocity( float fYaw, float fVelocity )
{
	if ( flags() & FLAG_CONTROL_DISABLED )
		return false;		// ship control is disabled
	// now set the new heading and velocity
	m_fLastSetHeading = m_fSetHeading = m_fHeading;
	m_fLastSetVelocity = m_fSetVelocity = Clamp( fVelocity, 0.0f, maxVelocity());
	m_fLastYaw = m_fYaw = Clamp( fYaw, -1.0f, 1.0f );

	return true;
}

bool NounShip::controlVelocity( float fVelocity )
{
	if ( flags() & FLAG_CONTROL_DISABLED )
		return false;		// ship control is disabled
	// now set the new velocity
	m_fLastSetVelocity = m_fSetVelocity = Clamp( fVelocity, 0.0f, maxVelocity());

	return true;
}

bool NounShip::controlHeading( float fHeading )
{
	if ( flags() & FLAG_CONTROL_DISABLED )
		return false;		// ship control is disabled

	// now set the new heading
	m_fLastSetHeading = m_fSetHeading = NormalizeAngle( fHeading );
	m_fLastYaw = m_fYaw = 0.0f;

	return true;
}

bool NounShip::controlYaw( float fYaw )
{
	if ( flags() & FLAG_CONTROL_DISABLED )
		return false;		// ship control is disabled

	// now set the new yaw
	m_fLastYaw = m_fYaw = Clamp( fYaw, -1.0f, 1.0f);
	m_fLastSetHeading = m_fSetHeading = m_fHeading;

	return true;
}

//----------------------------------------------------------------------------

// use all weapons linked to the spacebar
bool NounShip::verbAlpha( Noun * pTarget )
{
	bool bWeaponUsed = false;
	for(int i=0;i<childCount();i++)
	{
		NounGadget * pGadget = WidgetCast<NounGadget>( child(i) );
		if ( pGadget != NULL && pGadget->hotkey() == HK_SPACE )
		{
			useGadget( pGadget, pTarget, false );
			bWeaponUsed = true;
		}
	}

	return bWeaponUsed;
}

bool NounShip::verbBomb( Noun * pTarget )
{
	bool bWeaponUsed = false;
	for(int i=0;i<childCount();i++)
	{
		NounGadget * pGadget = WidgetCast<NounGadget>( child(i) );
		if ( pGadget != NULL && pGadget->type() == NounGadget::WEAPON_BOMB )
		{
			useGadget( pGadget, pTarget, false );
			bWeaponUsed = true;
		}
	}

	return bWeaponUsed;
}

void NounShip::useGadget( NounGadget * pGadget, Noun * pTarget, bool shift )
{
	// check that the ship is attached to the context, and that the gadget belongs to this ship
	if ( pGadget != NULL && context() != NULL && pGadget->parent() == this )		
	{
		if ( m_UseGadgets.size() > 0 && (pTarget != m_UseTarget || shift != m_UseShift) )
			useGadgets();	// flush the current use command now

		m_UseGadgets.push( pGadget );
		m_UseTarget = pTarget;
		m_UseShift = shift;
	}
}

bool NounShip::togglePointDefense()
{
	if (! isLocal() )
		return false;

	// determine the state of the beam weapons
	bool bState = false;
	
	Array< GadgetBeamWeapon * > weapons;
	for(int i=0;i<m_Gadgets.size();i++)
	{
		GadgetBeamWeapon * pBeam = WidgetCast<GadgetBeamWeapon>( m_Gadgets[i] );
		if ( pBeam != NULL )
		{
			bState |= pBeam->pointDefense();
			weapons.push( pBeam );
		}
	}

	for(int j=0;j<weapons.size();j++)
	{
		GadgetBeamWeapon * pBeam = weapons[j];
		if ( bState )
		{
			if ( pBeam->pointDefense() )
				useGadget( pBeam, NULL, true );		
		}
		else
		{
			if ( !pBeam->pointDefense() )
				useGadget( pBeam, NULL, true );
		}
	}

	return !bState;
}

bool NounShip::toggleShields()
{
	if (! isLocal() )
		return false;
	bool bState = false;

	for(int i=0;i<m_Shields.size();i++)
		bState |= m_Shields[i]->active();

	for(int j=0;j<m_Shields.size();j++)
	{
		GadgetShield * pShield = m_Shields[j];
		if ( bState && pShield->active() )
			useGadget( pShield, NULL, true );
		else if ( !bState && !pShield->active() )
			useGadget( pShield, NULL, true );
	}

	return !bState;
}

//----------------------------------------------------------------------------

const int MAX_MOVE_DEPTH = 10;

NounShip::MoveResult NounShip::moveToTarget( Noun * pTarget, float outside, float within, float speed, 
											bool bSlowToTarget, bool bForceJump, int nDepth /*= 0*/ )
{
	if (! isLocal() )
		return MOVE_ERROR;	
	if (! inLockedZone() )
		return MOVE_ERROR;		// ship is not in a locked zone
	if ( flags() & FLAG_CONTROL_DISABLED )
		return MOVE_ERROR;		// ship control is disabled

	Vector3 myPosition( worldPosition() );

	// check the distance to the target
	Vector3 delta( pTarget->worldPosition() - myPosition );
	float distance = delta.magnitudeXZ() - outside;

	if ( distance < within )
	{
		// if we are moving too fast, then we might need to turn to avoid a collision
		avoidCollision();
		return MOVE_DONE;
	}

	if ( nDepth < MAX_MOVE_DEPTH ) 
	{
		NounJumpGate * pGate = findJumpGate( pTarget );
		if ( pGate != NULL )
		{
			// we found a gate that will get us to our destination faster, so head towards the gate...
			return moveToTarget( pGate, 0.0f, 0.0f, 1.0f, false, false, nDepth + 1 );
		}

		GadgetJumpDrive * pJumpDrive = jumpDrive();
		if ( (bForceJump || distance > USE_JUMP_DISTANCE) && pJumpDrive != NULL )
		{
			if ( pJumpDrive->jumping() || pJumpDrive->engaged() )
				return MOVE_MOVING;

			if ( pJumpDrive->usable( NULL, true ) )
			{
				if ( distance < pJumpDrive->maxRange() )
				{
					float r2 = radius() * 2.0f;

					dword nNextJumpCheck = m_nLastJumpCheck + (CHECK_JUMP_INTERVAL * TICKS_PER_SECOND);
					if ( tick() > nNextJumpCheck || m_pLastJumpTarget != pTarget )
					{
						m_nLastJumpCheck = tick();
						m_pLastJumpTarget = pTarget;

						// check for a collision if we tried to jump
						Vector3 jumpTarget( myPosition + delta );
						Vector3 midPoint( (myPosition + jumpTarget) * 0.5f);

						// get a list of all objects that we might encounter
						Array< GameContext::NounCollision > nouns;
						context()->proximityCheck( midPoint, distance * 0.5f, nouns );

						// check for the first collision
						Noun * pCollide = NULL;
						float collideRange = distance;

						for(int i=0;i<nouns.size();i++)
						{
							Noun * pCheck = nouns[i].pNoun;
							if ( WidgetCast<NounBody>( pCheck ) == NULL || 
								WidgetCast<NounShip>( pCheck ) != NULL ||		// can't collide with other ships
								WidgetCast<NounNebula>( pCheck ) != NULL )		// can't collide with nebulas
							{
								// ignore object
								continue;
							}

							// ignore collision with our target
							if ( pCheck == pTarget )
								continue;

							Vector3 intersect;
							if ( pCheck->intersect( r2, myPosition, delta, intersect ) )
							{
								float range = (intersect - myPosition ).magnitude();
								if ( range < collideRange )
								{
									pCollide = pCheck;
									collideRange = range;
								}
							}
						}

						m_pLastJumpCollision = pCollide;
					}

					if ( m_pLastJumpCollision.valid() )
					{
						// avoid blowing the stack by checking if the target is already a NounTarget object
						if ( WidgetCast<NounTarget>( pTarget ) == NULL )
						{
							// object is obstructing a jump directly to pTarget, calculate a position on the outside of the collision
							// target and pushMove() to that position, calling this routine recursively FYI
							Vector3 normalDelta( delta );
							normalDelta.normalize();

							Vector3 collidePosition( m_pLastJumpCollision->worldPosition() );
							Vector3 collideDelta( collidePosition - myPosition );
							float collideRange = collideDelta.magnitude();
							float collideRadius = (m_pLastJumpCollision->radius() + r2) * 2.0f;		// add 50% to radius to make sure we clear the object
							collideDelta.normalize();

							Vector3 X( normalDelta * collideRange );
							Vector3 Y( collideDelta * collideRange );
							Vector3 Z( X - Y );
							Z.y = 0.0f;		// keep it 2D

							Z.normalize();

							// move to the new position
							NounZoneTarget::Ref rMoveTarget = new NounZoneTarget( collidePosition + (Z * collideRadius), context() );
							// call this routine recursively
							return moveToTarget( rMoveTarget, 0.0f, 0.0f, 1.0f, bSlowToTarget, bForceJump, nDepth + 1 );
						}

						// just fall through to the normal move code for now
					}
					else
					{
						// clear jump path to our target, jump to the target
						useGadget( pJumpDrive, pTarget, false );
						// send the verb now, otherwise the code may attempt to do it again
						useGadgets();

						return MOVE_MOVING;
					}
				} // if ( distance < pJumpDrive->maxRange() )
				else
				{
					// ship is out of jump range... 
					// TODO: Check fuel and send a supply distress signal if low...
				}
			} // pJumpDrive->usable( NULL, true )	
		} // if ( distance > USE_JUMP_DISTANCE && nDepth < MAX_JUMP_DEPTH && pJumpDrive != NULL )
	} // if ( nDepth < MAX_MOVE_DEPTH ) 

	// disabled this intercept code because it seemed that ships were over-compensating for the 
	// velocity of the target... perhaps the velocity is incorrect, but I'm unsure.
	//if ( WidgetCast<NounBody>( pTarget ) )
	//{
	//	delta = intercept( (NounBody *)pTarget, maxVelocity() * speed ) - myPosition;
	//	distance = delta.magnitudeXZ() - outside;
	//}
	distance -= within;

	//----------------------------------------------------------------------------

	// avoid objects
	float avoidRange = distance;
	float r2 = radius() * 2.0f;
	
	// how many times will we attempt to find a route to our destination
	int avoidCount = 4;
	while( avoidCount > 0 )
	{
		// enumerate through objects that are close, any objects that are between our position and our target position
		// need to be avoided, select the object that is closest, ignore additional objects
		Noun *	pCollide = NULL;
		for(int i=0;i<m_Contacts.size();i++)
		{
			Noun * pCheck = m_Contacts[i];

			if ( WidgetCast<NounBody>( pCheck ) == NULL || 
				WidgetCast<NounShip>( pCheck ) != NULL ||		// can't collide with other ships
				WidgetCast<NounNebula>( pCheck ) != NULL )		// can't collide with nebulas
			{
				// ignore object
				continue;
			}
			if ( WidgetCast<NounJumpGate>( pCheck ) != NULL && pCheck == pTarget )
				continue;										// don't avoid jump gates we're trying to use

			// check for intersection with object
			Vector3 intersect;
			if ( pCheck->intersect( r2, myPosition, delta, intersect ) )
			{
				float range = (intersect - myPosition ).magnitude();
				if ( range < avoidRange )
				{
					pCollide = pCheck;
					avoidRange = range;
				}
			}
		}

		// avoid object if we are going to collide
		if ( pCollide != NULL )
		{
			//TRACE( CharString().format("NounShip::pushMove, pCollide = %s, avoidCount = %d", pCollide->name(), avoidCount) );

			Vector3 normalDelta( delta );
			normalDelta.normalize();

			Vector3 collidePosition( pCollide->worldPosition() );
			Vector3 collideDelta( collidePosition - myPosition );
			float collideRange = collideDelta.magnitude();
			float collideRadius = (pCollide->radius() + r2) * 2.0f;		// add 50% to radius to make sure we clear the object
			collideDelta.normalize();

			//TRACE( CharString().format("NounFighter, avoiding collision with %s", pCollide->name() ) );

			// use the 2 normalized delta vectors, set their lengths to the distance from our position
			// to the collision target, then subtract those 2 vectors, normalize, and scale by the radius of the object
			Vector3 X( normalDelta * collideRange );
			Vector3 Y( collideDelta * collideRange );
			Vector3 Z( X - Y );
			Z.y = 0.0f;		// keep it 2D

			Z.normalize();

			//TRACE( CharString().format("X = %.1f,%.1f,%.1f, Y = %.1f,%.1f,%.1f, Z = %.1f,%.1f,%.1f",
			//	X.x,X.y,X.z, Y.x,Y.y,Y.z, Z.x,Z.y,Z.x) );

			// set the new target position
			Vector3 target( collidePosition + (Z * collideRadius) );
			// recalculate the delta vector
			delta = target - myPosition;

			// subtract one
			avoidCount--;
		}
		else
			avoidCount = -1;		
	}

	CTRACE( avoidCount == 0, "NounShip::move, failed to find path to target!" );

	//----------------------------------------------------------------------------

	// get the normalized delta, calculate the correct heading from that value
	Vector3 vNormalizedDelta( ~delta );
	// get the heading
	float fHeading = atan2( vNormalizedDelta.x, vNormalizedDelta.z );
	// calculate yaw delta
	float fYawDelta = AngleDelta( NormalizeAngle( m_fHeading ), NormalizeAngle( fHeading ) );
	// check the velocity
	float fAcceleration = maxAcceleration();

	if ( m_fVelocity > 0.0f )
	{
		// calculate time until destination
		float t = distance / m_fVelocity;
		// calculate how long it will take to stop
		float t2 = m_fVelocity / (fAcceleration * 2.0f);

		//TRACE( CharString().format("distance = %.1f, velocity = %.1f, a = %.1f, t = %.1f, t2 = %.1f", distance, m_fVelocity, acceleration, t, t2) );

		if ( t2 < t )
		{
			//TRACE( "NounShip::pushMove, t2 < t" );
			if ( fabs( fYawDelta ) < PI4 )
				controlHeadingVelocity( fHeading, maxVelocity() * speed ); 
			else
				controlHeading( fHeading ); 
		}
		else
		{
			//TRACE( "NounShip::pushMove, t2 > t" );
			if (! avoidCollision() )
			{
				if ( bSlowToTarget )
					controlVelocity( 0.0f );			// begin stopping
			}
		}
	}
	else
	{
		if ( fabs( fYawDelta ) < PI4 )
			controlHeadingVelocity( fHeading, maxVelocity() * speed ); //Min( m_fVelocity + acceleration,  ) );
		else
			controlHeading( fHeading );								// turn in place towards target
	}

	return MOVE_MOVING;
}

bool NounShip::updateDynamics( int nTicks /*= 1*/ )
{
	if (! isLocal() )
		return false;
	if ( nTicks <= 0 )
		return false;
	if ( inOrbit() )
	{
		if ( m_fSetVelocity > 0.0f && isLocal() )
			Verb::Ref( new VerbBreakOrbit( this ) );
	}

	PROFILE_START( "NounShip::updateDynamics()" );
	if ( (flags() & FLAG_DYNAMICS_DISABLED) )
	{
		float fElapsed = TICK_DURATION_S * nTicks;
		// if dynamics is disabled, decay the roll and y position if needed
		while( nTicks > 0 )
		{
			// decay the roll
			if ( enableRollDynamics() )
				m_fRoll -= m_fRoll / (mass() + 4.0f);		

			// decay the Y position to zero
			m_Position.y -= m_Position.y * 0.02f;
			nTicks--;
		}
#if ENABLE_SHIP_PITCH
		m_fPitch -= m_fPitch * fElapsed;		// decay any pitch
#endif

		// update the ship frame
		setOrientation( Vector3( m_fPitch, m_fHeading, -m_fRoll ) );
	}
	else
	{
		float fElapsed = TICK_DURATION_S * nTicks;
		float fMaxVelocity = maxVelocity();
		float fMaxYaw = maxYaw() * TICK_DURATION_S;
		float fMaxAcceleration = maxAcceleration() * TICK_DURATION_S;
		float fStaticYaw = m_fYaw * fMaxYaw;

		bool bInOrbit = inOrbit();
		while( nTicks > 0 )
		{
			// adjust the set heading by the current yaw
			m_fSetHeading += fStaticYaw;

			// move heading towards the set heading
			float fTurn = Clamp( AngleDelta( NormalizeAngle( m_fHeading ), NormalizeAngle( m_fSetHeading ) ), -fMaxYaw, fMaxYaw );
			if ( enableRollDynamics() )
			{
				m_fRoll = Clamp( m_fRoll - fTurn, MIN_SHIP_ROLL, MAX_SHIP_ROLL );
				m_fRoll -= m_fRoll / (mass() + 4.0f);		// decay the roll
			}
			// update the heading
			m_fHeading = NormalizeAngle( m_fHeading + fTurn );

			// get the acceleration
			float fAcceleration = Clamp( m_fSetVelocity - m_fVelocity, -fMaxAcceleration, fMaxAcceleration );
			// update the velocity
			m_fVelocity = Clamp( m_fVelocity + fAcceleration, 0.0f, fMaxVelocity );
			
			// move the ships position
			if ( m_fVelocity > 0.0f )
				m_Position += Vector3( sinf( m_fHeading ), 0.0f, cosf( m_fHeading) ) * (m_fVelocity * TICK_DURATION_S);
			// decay the Y position to zero
			m_Position.y -= m_Position.y * 0.02f;
			// decrement the ticks
			nTicks--;
		}

#if ENABLE_SHIP_PITCH
		// update the pitch...
		m_fPitch = Clamp( m_fPitch + (((m_LastPosition.y - m_Position.y) * fElapsed) * 0.5f), -PI4, PI4 );
		m_fPitch -= m_fPitch * fElapsed;		// decay any pitch
#endif

		//setFrame( Matrix33( 0.0f, m_fHeading, m_fRoll ) );
		setOrientation( Vector3( m_fPitch, m_fHeading, -m_fRoll ) );
		//setBaseVelocity( Vector3( sinf( m_fHeading ) * m_fVelocity, 0.0f, cosf( m_fHeading ) * m_fVelocity ) );
		// recalculate our world position now
		invalidateWorld();

	}
	PROFILE_END();

	return true;
}


//----------------------------------------------------------------------------
//EOF
